#include <Servo.h>
#include <SoftwareSerial.h>

const int SERVO_X_PIN = 10;
const int SERVO_Y_PIN = 9;
const int SERIAL_RX_PIN = 3;
const int SERIAL_TX_PIN = 4;
const int LED_PIN = 13;

const int SERVO_PLUS_BYTE = '+';
const int SERVO_MINUS_BYTE = '-';
const int SERVO_RESET_BYTE = '0';
const int SERVO_MOVE_STEP = 10;

int commandByte = 0;         // incoming serial byte

Servo servoX;
Servo servoY;

SoftwareSerial mySerial(SERIAL_RX_PIN, SERIAL_TX_PIN);

void setup(){
  // start serial port at 9600 bps:
  Serial.begin(9600);
  mySerial.begin(57600);
  pinMode(LED_PIN, OUTPUT);
  servoX.attach(SERVO_X_PIN);
  servoY.attach(SERVO_Y_PIN);
}

void loop(){
  if (Serial.available() > 0) {
    commandByte = Serial.read();
    log("Recieved: ");
    sendByteBack(commandByte);
// Platform control
// 1,2,3 - drive forward
// b - drive back
// l - rotate left
// r - rotate right
// q - stop any movement
// h - find dock station
//
// Camera control
// i - camera up
// k - camera down
// j - camera left
// l - camera right
// o - reset camera to 0

    switch (commandByte){
      case '1':
        driveForward(1);
        break;
      case '2':
        driveForward(2);
        break;
      case '3':
        driveForward(3);
        break;
      case 'l':
        rotateLeft();
        break;
      case 'b':
        driveBack();
        break;
      case 'r':
        rotateRight();
        break;
      case '0':
        platformStop();
        break;
      case 'h':
        platformHome();
        break;
      case 'y':
        platformReadSensors();
        break;
      case 'i':
        camUp();
        break;
      case 'k':
        camDown();
        break;
      case 'j':
        camLeft();
        break;
      case 'l':
        camRight();
        break;
      case 'o':
        camReset();
        break;
      case 'T':
        performTest();
        break;
      default:
        break;
    }
    delay(100);
  }
}

void driveForward(int i){
      platformStart();
      mySerial.write((byte)145);
      mySerial.write((byte)0);
      mySerial.write((byte)150);
      mySerial.write((byte)0);
      mySerial.write((byte)150);
};
void driveBack(){
      platformStart();
      mySerial.write((byte)145);
      mySerial.write((byte)255);
      mySerial.write((byte)200);
      mySerial.write((byte)255);
      mySerial.write((byte)200);
};
void rotateLeft(){
      platformStart();
      mySerial.write((byte)137);
      mySerial.write((byte)0);
      mySerial.write((byte)100);
      mySerial.write((byte)0);
      mySerial.write((byte)0);
};
void rotateRight(){
      platformStart();
      mySerial.write((byte)137);
      mySerial.write((byte)0);
      mySerial.write((byte)100);
      mySerial.write((byte)255);
      mySerial.write((byte)255);
};
void platformStop(){
      platformStart();
      mySerial.write((byte)137);
      mySerial.write((byte)0);
      mySerial.write((byte)0);
      mySerial.write((byte)0);
      mySerial.write((byte)0);
};
void platformHome(){
      mySerial.write((byte)128);
      mySerial.write((byte)136);
      mySerial.write((byte)9);
};

void platformStart(){
      mySerial.write((byte)128);
      mySerial.write((byte)132);
}

void platformReadSensors(){
      mySerial.write((byte)128);
      mySerial.write((byte)149);
      mySerial.write((byte)6);

      mySerial.write((byte)21);
      mySerial.write((byte)22);
      mySerial.write((byte)23);
      mySerial.write((byte)24);
      mySerial.write((byte)25);
      mySerial.write((byte)26);
      delay(100);
      
      byte sensorsByte[10];
      String sensorsString = "";
      int i = 0;
      while(mySerial.available() > 0){
        sensorsByte[i] = mySerial.read();
        sensorsString = sensorsString + " " + sensorsByte[i];
        i++;
      }
      log("Sensors raw data: " + sensorsString);
      log("Sensors[21]: " + convertBytesToString(sensorsByte[0]));
      log("Sensors[22]: " + convertBytesToStringUnsigned(sensorsByte[1], sensorsByte[2]) + " mV");
      log("Sensors[23]: " + convertBytesToStringSigned(sensorsByte[3], sensorsByte[4]) + " mA");
      log("Sensors[24]: " + convertBytesToString(sensorsByte[5]) + " C");
      log("Sensors[25]: " + convertBytesToStringUnsigned(sensorsByte[6], sensorsByte[7]) + " mAh");
      log("Sensors[26]: " + convertBytesToStringUnsigned(sensorsByte[8], sensorsByte[9]) + " mAh");
}

String convertBytesToStringSigned(byte hi, byte low){
  String result = "";
  result = result + (hi*256 + low);
  return result;
}

String convertBytesToStringUnsigned(byte hi, byte low){
  String result = "";
  result = result + (unsigned int)(hi*256 + low);
  return result;
}

String convertBytesToString(byte hi){
  String result = "";
  result = result + hi;
  return result;
}

void camUp(){
  moveServo(servoY, SERVO_PLUS_BYTE);
};
void camDown(){
  moveServo(servoY, SERVO_MINUS_BYTE);
};
void camLeft(){
  moveServo(servoX, SERVO_PLUS_BYTE);
};
void camRight(){
  moveServo(servoX, SERVO_MINUS_BYTE);
};
void camReset(){
  moveServo(servoX, SERVO_RESET_BYTE);
  moveServo(servoY, SERVO_RESET_BYTE);
};

void moveServo(Servo s, int commandByte){
  int currentPos = s.read();
  log("CurrentPos: ");
  sendByteBack(currentPos);
  int nextPos = 90;
  switch (commandByte){
    case SERVO_PLUS_BYTE:
      nextPos = currentPos + SERVO_MOVE_STEP;
      if (nextPos > 180) nextPos = 180;
      break;
    case SERVO_MINUS_BYTE:
      nextPos = currentPos - SERVO_MOVE_STEP;
      if (nextPos < 0) nextPos = 0;
      break;
  }
  log("NextPos: ");
  sendByteBack(nextPos);
  s.write(nextPos);
  delay(300);
}


void blink(){
  digitalWrite(LED_PIN, HIGH);   
  delay(500);              
  digitalWrite(LED_PIN, LOW);    
  delay(500); 
}

void sendByteBack(int inByte){
  Serial.write(inByte);
}

void log(String msg){
  Serial.println(msg);
}

void performTest(){
  
  log("Arduino blink 2 times");
  blink();
  blink();

  log("Test Camera movement");
  camUp();
  delay(1000);
  camDown();
  delay(1000);
  camLeft();
  delay(1000);
  camRight();
  delay(1000);

  log("Arduino blink 3 times");
  blink();
  blink();
  blink();

  log("Test Complete");
}
